/*
 * distributor_emulator.c
 *
 *  Created on: Oct 9, 2012
 *      Author: andrey
 */

#include "distributor_emulator.h"
#include "io.h"
#include "drawing.h"
#include "rficonsole.h"
#include "rfirtc.h"

//static WORKING_AREA(deThread2Stack, 256);

int limpingWaveFreq = 80;
#define adcFreq  130
int waveDuty = 50;

systime_t getWaveLength() {
	return MS2ST(1000 / limpingWaveFreq) ;
}

int getWaveFreq() {
	return limpingWaveFreq;
}

void setWaveFreq(int value) {
	limpingWaveFreq = value;
}

#define max(a, b) ((a) > (b) ? (a) : (b))

#define TEST_TIMES_COUNT 500
time_t test_times[TEST_TIMES_COUNT];
time_t pin_result[TEST_TIMES_COUNT];
time_t mstimes[TEST_TIMES_COUNT];

int de_counter = 0;

void print_times() {
	for (int i = 1; i < TEST_TIMES_COUNT; i++) {
		print("t %d %d(%d): %d\r\n", i, test_times[i],
				test_times[i] - test_times[i - 1], pin_result[i]);
		if (i == 1 || i == TEST_TIMES_COUNT - 1)
			print("%d %d\r\n", test_times[i], mstimes[i]);
	}

	print("tick %d\r\n", test_times[TEST_TIMES_COUNT - 1] - test_times[1]);
	print("ms %d\r\n", mstimes[TEST_TIMES_COUNT - 1] - mstimes[1]);
}

static msg_t deThread22(void *arg) {
	chRegSetThreadName("Wave");

	addConsoleAction("times", &print_times);

	for (int i = 0; i < TEST_TIMES_COUNT; i++) {
		test_times[i] = 0;
		mstimes[i] = 0;
	}

	time_t start_time = chTimeNow();



//	int maxFreq = max(limpingWaveFreq, gRpm);
//	long sleep = CH_FREQUENCY / maxFreq / 100;

//	myfloat adcWave = freq2wave(100);
//	myfloat limpingWave1 = freq2wave(limpingWaveFreq);
//	myfloat limpingWave2 = freq2wave(2 * limpingWaveFreq);

	while (TRUE) {

		de_counter++;

//		time_t now = chTimeNow();
//		int wave1 = waveState(start_time, now, limpingWave1, 0, 0.5);
//		if (de_counter >= 500 && de_counter < 500 + TEST_TIMES_COUNT) {
//			int index = de_counter - 500;
//			test_times[index] = now;
//			pin_result[index] = wave1;
//			if (index == 1 || index == TEST_TIMES_COUNT - 1)
//				mstimes[index] = currentTimeMillis();
//		}

//		int adcPin = waveState(start_time, now, adcWave, 0, 0.5);
//		setPinValue(DE_ADC_IO_PORT, DE_ADC_WAVE_PIN, adcPin);

//		setPinValue(DE_IO_PORT, DE_WAVE_1_PIN, wave1);

//		int wave2 = waveState(start_time, now, limpingWave2, 0.5, 1);
//		setPinValue(DE_IO_PORT, DE_WAVE_2_PIN, wave2);

//		int distrG = waveState(start_time, now, freq2wave(4 * gRpm), 0, distrGRight);
		// not sure if this is right or should be inverted
//		int distr = waveState(start_time, now, freq2wave(gRpm), 0, distrRight);

//		setPinValue(DE_IO_PORT, DE_WHITE_WIRE_PIN, deEnabled && distrG);
//		setPinValue(DE_IO_PORT, DE_Y_BL_WIRE_PIN, !distr);

//		myassert(sleep > 0, "sleep");
		chThdSleep(1);
	}
}

//void initDistributorEmulator() {
//	palSetGroupMode(DE_IO_PORT,
//			PAL_PORT_BIT(DE_WHITE_WIRE_PIN) | PAL_PORT_BIT(DE_Y_BL_WIRE_PIN) | PAL_PORT_BIT(DE_WAVE_1_PIN) | PAL_PORT_BIT(DE_WAVE_2_PIN)
////			| PAL_PORT_BIT(DE_PA13)
//			, 0, PAL_MODE_OUTPUT_PUSHPULL);
//
//	palSetGroupMode(DE_ADC_IO_PORT, PAL_PORT_BIT(DE_ADC_WAVE_PIN), 0,
//			PAL_MODE_OUTPUT_PUSHPULL);
//
//	chThdCreateStatic(deThread2Stack, sizeof(deThread2Stack), NORMALPRIO,
//			deThread2, NULL );
//}
